#include <map_load/map_load_node.h>

/******************************************************
 * 加载此功能包maps文件夹下地图数据 * 
 用法：rosrun map_load map_load_node {map_name}
 例如：rosrun map_load map_load_node 4F
 ******************************************************/


MapLoadNode::MapLoadNode(){
    __map_pub = __nh.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
}


MapLoadNode::~MapLoadNode(){
    
}

// 读取Yaml文件
YAML::Node MapLoadNode::ReadYamlFile(string file_path){
    YAML::Node config;
    try{
        config = YAML::LoadFile(file_path);
    }catch(YAML::BadFile &e) {
        std::cout << "read error, file_path:" << file_path.c_str() << std::endl;
    }
    return config;
}

// Euler(roll, pitch, yaw) -> tf2::Quaternion
tf2::Quaternion MapLoadNode::Euler2TFQuaternion(double roll, double pitch, double yaw){
    tf2::Quaternion tf2_quat;
    tf2_quat.setRPY(roll, pitch, yaw);
    return tf2_quat;
}

// Euler(roll, pitch, yaw) -> geometry_msgs::Quaternion
geometry_msgs::Quaternion MapLoadNode::Euler2GeometryQuaternion(double roll, double pitch, double yaw){
    tf2::Quaternion tf2_quat = Euler2TFQuaternion(roll, pitch, yaw);
    geometry_msgs::Quaternion geom_quat = tf2::toMsg(tf2_quat);
    return geom_quat;
}

// 获取地图基础信息 从map.yaml文件中
nav_msgs::MapMetaData MapLoadNode::MapMetaDataFromYaml(string map_yaml_file){
    nav_msgs::MapMetaData metadata;
    YAML::Node map_yaml_info = ReadYamlFile(map_yaml_file);
    if(!map_yaml_info.IsNull()){
        if(map_yaml_info["height"])
            metadata.height = map_yaml_info["height"].as<int>();
        if(map_yaml_info["width"])
            metadata.width = map_yaml_info["width"].as<int>();
        if(map_yaml_info["resolution"])
            metadata.resolution = map_yaml_info["resolution"].as<float>();
        if(map_yaml_info["origin_x"])
            metadata.origin.position.x = map_yaml_info["origin_x"].as<float>();
        if(map_yaml_info["origin_y"])
            metadata.origin.position.y = map_yaml_info["origin_y"].as<float>();
        if(map_yaml_info["origin_yaw"]){
            float yaw = map_yaml_info["origin_yaw"].as<float>();
            metadata.origin.orientation = Euler2GeometryQuaternion(0.0, 0.0, yaw);
        }
    }
    return metadata;
}

// 加载地图
void MapLoadNode::MapLoad(string map_name){
    std::cout << "[MapLoad] map_name: " << map_name.c_str() << std::endl;
    string package_path = ros::package::getPath("map_load");
    std::cout << "[MapLoad] package_path: " << package_path.c_str() << std::endl;
    string map_dir = package_path + "/maps/" + map_name + "/";
    std::cout << "[MapLoad] map_dir: " << map_dir.c_str() << std::endl;
    string map_png_file = map_dir + "map.png";
    string map_yaml_file = map_dir + "map.yaml";

    // 获取地图元数据
    nav_msgs::MapMetaData metadata = MapMetaDataFromYaml(map_yaml_file);

    int __threshold_l = 80;         // 过滤黑色像素
    int __threshold_h = 130;        // 过滤灰色白色像素

    cv::Mat map_image = cv::imread(map_png_file, 0);
    nav_msgs::OccupancyGrid map_msg;
    map_msg.header.frame_id = "map";
    map_msg.header.stamp = ros::Time::now();
    map_msg.info = metadata;
    map_msg.data.resize(map_msg.info.height * map_msg.info.width);
    for(int h = 0; h < map_msg.info.height; h++){
        for(int w = 0; w < map_msg.info.width; w++){
            int index = (map_msg.info.height - h - 1) * map_msg.info.width + w;
            int data = map_image.at<uchar>(h, w);
            if(data > __threshold_h){   // >220  白色 
                map_msg.data[index] = 0;
            }
            else if(data < __threshold_l){  // <30 黑色
                map_msg.data[index] = 100;
            }
            else{
                map_msg.data[index] = -1;
            }
        }
    }
    __map_pub.publish(map_msg);  
}

/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "map_load_node");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
	
    ROS_INFO("map_load_node initializating......");
    ros::NodeHandle nh;

    // 打印参数
    for(int i=0; i<argc; i++){
        std::cout << "i: " << i << ", data: " << argv[i] << std::endl;
    }
    if(argc < 2){
        std::cout << "please set map_name."  << std::endl;
        return 1;
    }

    MapLoadNode map_load_node;
    map_load_node.MapLoad(argv[1]);
    ros::spin();
    return 0;
}